﻿using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// 2D版GJK碰撞检测库
/// </summary>
public static class GJK2DEngine
{
    /// <summary>
    /// 迭代次数
    /// </summary>
    public static int LoopIterNum = 20;

    private const float Tolerance = 0.000001f;

    public struct SupportPoint
    {
        public Vector2 point; //明可夫斯基点
        public Vector2 fromA; //A集合点
        public Vector2 fromB; //B集合点

        public Vector2 GetPoint()
        {
            return point;
        }
    }

    public class Edge
    {
        public SupportPoint a; //a点
        public SupportPoint b;//b点
        public Vector2 normal;//边的法线方向
        public float distance;//边到原点距离
        public int index;//序号
    }

    public class SimplexEdge
    {
        public List<Edge> edges = new List<Edge>();

        public void clear()
        {
            edges.Clear();
        }
        public void initEdges(List<SupportPoint> simplex)
        {
            edges.Clear();

            int n = simplex.Count;
            for (int i = 0; i < n; ++i)
            {
                int iNext = (i + 1) % n;
                Edge edge = createEdge(simplex[i], simplex[iNext]);
                edges.Add(edge);
            }

            updateEdgeIndex();
        }

        public Edge findClosestEdge()
        {
            float minDistance = float.MaxValue;
            Edge ret = null;
            foreach (var e in edges)
            {
                if (e.distance < minDistance)
                {
                    ret = e;
                    minDistance = e.distance;
                }
            }
            return ret;
        }

        public void insertEdgePoint(Edge e, SupportPoint point)
        {
            Edge e1 = createEdge(e.a, point);
            edges[e.index] = e1;
            Edge e2 = createEdge(point, e.b);
            edges.Insert(e.index + 1, e2);
            updateEdgeIndex();
        }

        public void updateEdgeIndex()
        {
            for (int i = 0; i < edges.Count; ++i)
            {
                edges[i].index = i;
            }
        }

        public Edge createEdge(SupportPoint a, SupportPoint b)
        {
            Edge e = new Edge();
            e.a = a;
            e.b = b;

            e.normal = GetPerpendicularToOrigin(a.point, b.point, Vector2.zero);
            float lengthSq = e.normal.sqrMagnitude;
            // 单位化边
            if (lengthSq > 0.0001f)
            {
                e.distance = Mathf.Sqrt(lengthSq);
                e.normal *= 1.0f / e.distance;
            }
            else
            {
                // 如果距离原点太近，用数学的方法来得到直线的垂线
                // 方向可以随便取，刚好另外一边是反着来的
                Vector2 v = a.point - b.point;
                v.Normalize();
                e.normal = new Vector2(-v.y, v.x);
            }
            return e;
        }
    }

    /// <summary>
    /// 判断两个凸包围盒是否碰撞
    /// </summary>
    /// <param name="shapeA"></param>
    /// <param name="shapeB"></param>
    /// <returns></returns>
    public static bool IsContract(List<Vector2> shapeA, List<Vector2> shapeB, out Vector2 AClosestPonit, out Vector2 BClosestPoint, out List<SupportPoint> simplex, out Vector2 EPADir, bool isCatchClosePoint = false)
    {
        EPADir = Vector2.zero;
        AClosestPonit = Vector2.zero;
        BClosestPoint = Vector2.zero;
        bool IsCollision = false;
        //List<SupportPoint> simplex = new List<SupportPoint
        simplex = new List<SupportPoint>();
        Vector2 direction = GetFirstDirection(0, 0, shapeA, shapeB);//获取方向向量
        simplex.Add(Support(direction, shapeA, shapeB));//获取该方向上第一个交集
        simplex.Add(Support(-direction, shapeA, shapeB));//获取第二个明克夫斯基差
        direction = -GetPerpendicularToOrigin(simplex[1].GetPoint(), simplex[0].GetPoint(), Vector2.zero);//指向原点的向量
        for (int i = 0; i < LoopIterNum; i++)
        {
            // 方向接近于0，说明原点就在边上
            if (direction.sqrMagnitude <= Tolerance)
            {
                IsCollision = true;
                break;
            }
            SupportPoint p = Support(direction, shapeA, shapeB);
            // 新点与之前的点重合了。也就是沿着dir的方向，已经找不到更近的点了。
            if ((p.GetPoint() - simplex[0].GetPoint()).sqrMagnitude < Tolerance || (p.GetPoint() - simplex[1].GetPoint()).sqrMagnitude < Tolerance)
            {
                IsCollision = false;
                break;

            }
            simplex.Add(p);//获取第三个明克夫斯基差

            //TODO 判断当前是否包含原点
            if (ContainPoint(simplex, Vector2.zero))
            {
                IsCollision = true;
                break;
            }
            //TODO 重新寻找离原点最近的两个点
            direction = FindNextDirection(simplex, shapeA, shapeB);//都是指向原点方向向量
        }

        if (!IsCollision && isCatchClosePoint)
        {
            GetCollisionPoint(simplex, ref AClosestPonit, ref BClosestPoint);
        }
        else
        {
            EPADir = QueryEPA(shapeA, shapeB, simplex);
        }
        return IsCollision;
    }

    private static Vector2 QueryEPA(List<Vector2> shapeA, List<Vector2> shapeB, List<SupportPoint> simplex)
    {
        SimplexEdge simplexEdge = new SimplexEdge();
        simplexEdge.initEdges(simplex);// 初始化边数据，// 构造一个首尾相连的多边形
        Vector2 penetrationVector = Vector2.zero;
        Edge currentEpaEdge = null;
        for (int i = 0; i < LoopIterNum; ++i)
        {
            Edge e = simplexEdge.findClosestEdge();//找到最近的边
            currentEpaEdge = e;
            if (e == null)
            {
                return penetrationVector;
            }
            // 沿着边的法线方向，尝试找一个新的support点
            penetrationVector = e.normal * e.distance;
            // 沿着边的法线方向，尝试找一个新的support点
            SupportPoint point = Support(e.normal, shapeA, shapeB);
            float distance = Vector2.Dot(point.point, e.normal);
            if (distance - e.distance < Tolerance)
            {
                // 返回穿透向量
                return penetrationVector = e.normal * distance;
            }
            // 将新的support点插入到多边形中。
            // 也就是将边e从support点位置分割成两条新的边，并替换到多边形集合中。
            simplexEdge.insertEdgePoint(e, point);
        }
        return penetrationVector;
    }

    private static void GetCollisionPoint(List<SupportPoint> simplex, ref Vector2 closestOnA, ref Vector2 closestOnB)
    {
        /*
         *  L = AB，是Minkowski差集上的一个边，同时构成A、B两点的顶点也来自各自shape的边。
         *  E1 = Aa - Ba，E2 = Ab - Bb
         *  则求两个凸包的最近距离，就演变成了求E1和E2两个边的最近距离。
         *  
         *  设Q点是原点到L的垂点，则有:
         *      L = B - A
         *      Q · L = 0
         *  因为Q是L上的点，可以用r1, r2来表示Q (r1 + r2 = 1)，则有: Q = A * r1 + B * r2
         *      (A * r1 + B * r2) · L = 0
         *  用r2代替r1: r1 = 1 - r2
         *      (A - A * r2 + B * r2) · L = 0
         *      (A + (B - A) * r2) · L = 0
         *      L · A + L · L * r2 = 0
         *      r2 = -(L · A) / (L · L)
        */
        SupportPoint A = simplex[0];
        SupportPoint B = simplex[1];

        Vector2 L = B.GetPoint() - A.GetPoint();
        float sqrDistanceL = L.sqrMagnitude;
        // support点重合了
        if (sqrDistanceL < Tolerance)
        {
            closestOnA = closestOnB = A.GetPoint();
        }
        else
        {
            float r2 = -Vector2.Dot(L, A.GetPoint()) / sqrDistanceL;
            r2 = Mathf.Clamp01(r2);
            float r1 = 1.0f - r2;

            closestOnA = A.fromA * r1 + B.fromA * r2;
            closestOnB = A.fromB * r1 + B.fromB * r2;
        }
    }

    private static Vector2 FindNextDirection(List<SupportPoint> simplex, List<Vector2> shapeA, List<Vector2> shapeB)
    {

        if (simplex.Count == 2)
        {
            Vector2 crossPoint = GetPerpendicularToOrigin(simplex[0].GetPoint(), simplex[1].GetPoint(), Vector2.zero);
            return crossPoint;

        }
        else if (simplex.Count == 3)
        {
            Vector2 crossOnCA = GetPerpendicularToOrigin(simplex[2].GetPoint(), simplex[0].GetPoint(), Vector2.zero);
            Vector2 crossOnCB = GetPerpendicularToOrigin(simplex[2].GetPoint(), simplex[1].GetPoint(), Vector2.zero);
            // 保留距离原点近的，移除较远的那个点
            if (crossOnCA.sqrMagnitude < crossOnCB.sqrMagnitude)
            {
                simplex.RemoveAt(1);
                return Vector2.zero - crossOnCA;
            }
            else
            {
                simplex.RemoveAt(0);
                return Vector2.zero - crossOnCB;
            }
        }
        else
        {
            //error
            Debug.Log("FindNextDirection simplex count is beyond 3 !");
            return Vector2.zero;
        }

    }

    /// <summary>
    /// 寻找的原点到向量的垂足点位置
    /// </summary>
    private static Vector2 GetPerpendicularToOrigin(Vector2 a, Vector2 b, Vector2 original)
    {
        Vector2 ab = b - a;
        if (ab.sqrMagnitude == 0)
        {//a、b两点重合，随机返回
            return a;
        }
        Vector2 ao = original - a;
        float projection = Vector2.Dot(ab, ao) / ab.sqrMagnitude;
        if (projection < 0)//投影越界
        {
            return a;
        }
        else if (projection > 1)//投影越界
        {
            return b;

        }
        else
        {
            return a + ab * projection;
        }
    }

    /// <summary>
    /// 判断当前形状是否包含该点（三角形）
    /// </summary>
    /// <param name="shapePiontList"></param>
    /// <param name="point"></param>
    private static bool ContainPoint(List<SupportPoint> shapePiontList, Vector2 point)
    {
        int n = shapePiontList.Count;
        if (n < 3)
        {
            return false;
        }

        Vector2 a = shapePiontList[0].GetPoint();
        Vector2 b = shapePiontList[1].GetPoint();
        Vector2 c = shapePiontList[2].GetPoint();
        int inside = WhitchSide(a, b, c);
        for (int i = 0; i < shapePiontList.Count; ++i)
        {
            int side = WhitchSide(shapePiontList[i].GetPoint(), shapePiontList[(i + 1) % shapePiontList.Count].GetPoint(), point);
            if (side == 0)
            {
                return true;
            }
            if (inside != side)
            {//点都必须在所有边的同一侧
                return false;
            }
        }
        return true;
    }



    private static Vector2 GetFirstDirection(int indexA, int indexB, List<Vector2> a, List<Vector2> b)
    {
        Vector2 aP = a[indexA];
        Vector2 bP = b[indexB];
        Vector2 tempDir = aP - bP;
        if (tempDir.magnitude <= 0)
        {
            return GetFirstDirection(indexA + 1, indexB, a, b);
        }
        else
        {
            return tempDir;
        }
    }



    /// <summary>
    /// 获取明可夫斯基之差向量
    /// </summary>
    /// <returns></returns>
    private static SupportPoint Support(Vector2 dir, List<Vector2> A, List<Vector2> B)
    {
        Vector2 a = GetFarthestPointInDirection(dir, A);
        Vector2 b = GetFarthestPointInDirection(-dir, B);
        return new SupportPoint()
        {
            point = a - b,
            fromA = a,
            fromB = b
        };
    }

    private static Vector2 GetFarthestPointInDirection(Vector2 dir, List<Vector2> pointList)
    {

        int maxPointIndex = 0;
        float maxValue = float.MinValue;
        for (int i = 0; i < pointList.Count; i++)
        {
            float value = Vector2.Dot(pointList[i], dir);
            if (maxValue < value)
            {
                maxValue = value;
                maxPointIndex = i;
            }
        }
        return pointList[maxPointIndex];
    }


    /// <summary>
    /// 判断C点在a、b两点的哪一侧，利用叉乘来判断
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="c"></param>
    /// <returns>0刚好在ab线上，1 在向量左边，2在向量右边</returns>
    public static int WhitchSide(Vector2 a, Vector2 b, Vector2 c)
    {
        Vector2 ab = b - a;
        Vector2 ac = c - a;
        float cross = ab.x * ac.y - ab.y * ac.x;
        return cross > 0 ? 1 : (cross < 0 ? -1 : 0);
    }
}